﻿/*
 * SkeletonDataParser.cs
 * 
 * Manages reading/writing of skeleton data. A human-readable text file
 * is used (default ".txt") to serialize the skeleton data. Additional
 * comma-separated values file (default ".csv") are produced for MATLAB use.
 * 
 * Please note, the format of the files is as follows.
 * 
 * Arm Angles:
 * [TIMESTAMP] LShoulderRoll LShoulderPitch LElbowRoll LElbowYaw RShoulderRoll RShoulderPitch RElbowRoll RElbowYaw
 * 
 * Torso Position:
 * [TIMESTAMP] RelativeX RelativeY RelativeZ
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using grounding.nao.enums;

namespace grounding.nao
{
    enum SkeletonDataType
    {
        ArmAngles = 0,
        TorsoPositions
    };
    
    struct Position
    {
        public double x, y, z;
    };

    class SkeletonDataParser
    {
        private const string SERIALIZATION_FOLDER = @"../../SkeletonData/"; // Default = "../../SkeletonData/"
        private const string SERIALIZATION_PREFIX = @"SkeletonData_"; // Default = "SkeletonData_"
        private const string SERIALIZATION_SUFFIX = @".txt"; // Default = ".txt"
        private const string SERIALIZATION_COMMENT = @"//"; // Default = "//"
        private const string SERIALIZATION_CSV_SUFFIX = @".csv"; // Default = ".csv"
        private const string SERIALIZATION_CSV_COMMENT = @"%"; // Default = "%"
        private const string RECORDING_DELIMETER = @"END_MOTION"; // Default = "END_MOTION"

        private const uint SAMPLING_DELTA = 200; // Take samples no more often than this interval (ms).
        
        private string name;
        private SkeletonDataType type;
        private bool currentlyCapturing;
        private Dictionary<Motor, List<KeyValuePair<DateTime, float>>> currentAngleDictionary;
        private List<KeyValuePair<DateTime, Position>> currentPositionList;
        private long lastCaptureTick;

        // Maintain a list of dictionaries of arm joint -> a list of its angles corresponding with time.
        private List<Dictionary<Motor, List<KeyValuePair<DateTime, float>>>> angles;

        // Maintain a list of lists of torso position over time.
        private List<List<KeyValuePair<DateTime, Position>>> positions;

        public SkeletonDataParser(string name, SkeletonDataType type)
        {
            angles = new List<Dictionary<Motor, List<KeyValuePair<DateTime, float>>>>();
            positions = new List<List<KeyValuePair<DateTime, Position>>>();

            this.name = name;
            this.type = type;
            this.currentlyCapturing = false;
            this.lastCaptureTick = 0;
        }

        /// <summary>
        /// Begin recording a single motion.
        /// </summary>
        public void BeginMotionCapture()
        {
            if (currentlyCapturing)
                throw new Exception("Already recording motion.");

            if (SkeletonDataType.ArmAngles == type)
            {
                currentAngleDictionary = new Dictionary<Motor, List<KeyValuePair<DateTime, float>>>();
                angles.Add(currentAngleDictionary);
            }
            else
            {
                currentPositionList = new List<KeyValuePair<DateTime, Position>>();
                positions.Add(currentPositionList);
            }

            currentlyCapturing = true;
        }

        /// <summary>
        /// Append the given arm angle to the capture data.
        /// </summary>
        /// <param name="angles">dictionary of Motor enum -> angles in radians</param>
        /// <param name="d">time of record</param>
        /// <param name="ignoreTime">ignore the SAMPLING_DELTA constraint</param>
        private void AppendArmJointAngles(Dictionary<Motor, float> angles, DateTime d, bool ignoreTime)
        {
            if (SkeletonDataType.ArmAngles != type)
                throw new Exception("Not currently expecting arm angles.");

            if (!currentlyCapturing)
                throw new Exception("Not currently recording motion.");

            if (ignoreTime || 0 == lastCaptureTick || System.Environment.TickCount - lastCaptureTick > 200)
            {
                if (!ignoreTime)
                    lastCaptureTick = System.Environment.TickCount;

                foreach (Motor m in angles.Keys)
                {
                    if (!currentAngleDictionary.ContainsKey(m))
                        currentAngleDictionary.Add(m, new List<KeyValuePair<DateTime, float>>());

                    // Add the current time and motor angle to the dictionary.
                    currentAngleDictionary[m].Add(new KeyValuePair<DateTime, float>(d, angles[m]));
                }
            }
        }

        /// <summary>
        /// Append the given arm angle to the capture data.
        /// </summary>
        /// <param name="angles">dictionary of Motor enum -> angles in radians</param>
        public void AppendArmJointAngles(Dictionary<Motor, float> angles)
        {
            AppendArmJointAngles(angles, DateTime.Now, false);
        }

        /// <summary>
        /// Append the given torso position to the capture data.
        /// </summary>
        /// <param name="p">the relative position of the torso</param>
        /// <param name="d">time of record</param>
        /// <param name="ignoreTime">ignore the SAMPLING_DELTA constraint</param>
        private void AppendTorsoPosition(Position p, DateTime d, bool ignoreTime)
        {
            if (SkeletonDataType.TorsoPositions != type)
                throw new Exception("Not currently expecting torso positions.");

            if (!currentlyCapturing)
                throw new Exception("Not currently recording motion.");

            if (ignoreTime || 0 == lastCaptureTick || System.Environment.TickCount - lastCaptureTick > 200)
            {
                if (!ignoreTime)
                    lastCaptureTick = System.Environment.TickCount;

                currentPositionList.Add(new KeyValuePair<DateTime, Position>(d, p));
            }
        }

        /// <summary>
        /// Append the given torso position to the capture data.
        /// </summary>
        /// <param name="p">the relative position of the torso</param>
        public void AppendTorsoPosition(Position p)
        {
            AppendTorsoPosition(p, DateTime.Now, false);
        }

        /// <summary>
        /// Returns whether or not the skeleton parser is currently capturing.
        /// </summary>
        /// <returns>true if capturing; false otherwise.</returns>
        public bool IsCurrentlyCapturing()
        {
            return currentlyCapturing;
        }

        /// <summary>
        /// Finish recording a single motion.
        /// </summary>
        public void FinishMotionCapture()
        {
            if (!currentlyCapturing)
                throw new Exception("Not currently recording motion.");

            lastCaptureTick = 0;
            currentlyCapturing = false;
        }

        /// <summary>
        /// Return a deep-clone of the current set of arm angle samples.
        /// </summary>
        /// <returns>a list of dictionaries of arm joint -> a list of its angles corresponding with time</returns>
        public List<Dictionary<Motor, List<KeyValuePair<DateTime, float>>>> fetchArmAngles()
        {
            if (SkeletonDataType.ArmAngles != type)
                throw new Exception("The data type is: " + type.ToString());

            if (currentlyCapturing)
                throw new Exception("Currently recording motion.");

            List<Dictionary<Motor, List<KeyValuePair<DateTime, float>>>> anglesNew = new List<Dictionary<Motor, List<KeyValuePair<DateTime, float>>>>();

            foreach (Dictionary<Motor, List<KeyValuePair<DateTime, float>>> dOld in angles)
            {
                Dictionary<Motor, List<KeyValuePair<DateTime, float>>> dNew = new Dictionary<Motor, List<KeyValuePair<DateTime, float>>>();

                foreach (Motor m in dOld.Keys)
                {
                    List<KeyValuePair<DateTime, float>> lNew = new List<KeyValuePair<DateTime, float>>();

                    foreach (KeyValuePair<DateTime, float> kOld in dOld[m])
                    {
                        // Add this time/angle for this joint.
                        lNew.Add(new KeyValuePair<DateTime, float>(kOld.Key, kOld.Value));
                    }

                    // Add this list to the dictionary.
                    dNew[m] = lNew;
                }

                // Add this recording sample to the list of samples.
                anglesNew.Add(dNew);
            }

            return anglesNew;
        }

        /// <summary>
        /// Return a deep-clone of the current set of torso position samples.
        /// </summary>
        /// <returns>a list of lists of torso position over time.</returns>
        public List<List<KeyValuePair<DateTime, Position>>> fetchTorsoPositions()
        {
            if (SkeletonDataType.TorsoPositions != type)
                throw new Exception("The data type is: " + type.ToString());

            if (currentlyCapturing)
                throw new Exception("Currently recording motion.");

            List<List<KeyValuePair<DateTime, Position>>> positionsNew = new List<List<KeyValuePair<DateTime, Position>>>();

            foreach (List<KeyValuePair<DateTime, Position>> lOld in positions)
            {
                List<KeyValuePair<DateTime, Position>> lNew = new List<KeyValuePair<DateTime, Position>>();

                foreach (KeyValuePair<DateTime, Position> kOld in lOld)
                {
                    // Add the time/position for this frame.
                    lNew.Add(new KeyValuePair<DateTime, Position>(kOld.Key, kOld.Value));
                }

                // Add this recording sample to the list of samples.
                positionsNew.Add(lNew);
            }

            return positionsNew;
        }

        /// <summary>
        /// Return a deep-clone of the specified set of arm angle samples.
        /// </summary>
        /// <param name="g">which gesture to fetch data for</param>
        /// <returns>a list of dictionaries of arm joint -> a list of its angles corresponding with time</returns>
        public List<Dictionary<Motor, List<KeyValuePair<DateTime, float>>>> fetchArmAngles(string g)
        {
            if (currentlyCapturing)
                throw new Exception("Currently recording motion.");

            // Clear old data.
            angles = null;
            positions = null;

            // Prepare for new data.
            angles = new List<Dictionary<Motor, List<KeyValuePair<DateTime, float>>>>();
            positions = new List<List<KeyValuePair<DateTime, Position>>>();

            type = SkeletonDataType.ArmAngles;
            name = g;

            LoadSkeletonData();

            return fetchArmAngles();
        }

        /// <summary>
        /// Return a deep-clone of the current set of torso position samples.
        /// </summary>
        /// <param name="g">which gesture to fetch data for</param>
        /// <returns>a list of lists of torso position over time.</returns>
        public List<List<KeyValuePair<DateTime, Position>>> fetchTorsoPositions(string g)
        {
            if (currentlyCapturing)
                throw new Exception("Currently recording motion.");

            // Clear old data.
            angles = null;
            positions = null;

            // Prepare for new data.
            angles = new List<Dictionary<Motor, List<KeyValuePair<DateTime, float>>>>();
            positions = new List<List<KeyValuePair<DateTime, Position>>>();

            type = SkeletonDataType.TorsoPositions;
            name = g;

            LoadSkeletonData();

            return fetchTorsoPositions();
        }

        /// <summary>
        /// Write the current skeleton data to disk.
        /// 
        /// Precondition:
        /// All 8 arm angles must be present in each dictionary with each
        /// entry containing the same number of angles saved.
        /// </summary>
        public void WriteSkeletonData()
        {
            if (currentlyCapturing)
                throw new Exception("Currently capturing data.");

            uint csvIndex = 1;

            // Verify that serialization folder exists.
            if (!Directory.Exists(SERIALIZATION_FOLDER))
            {
                // If it doesn't, create it.
                Directory.CreateDirectory(SERIALIZATION_FOLDER);
            }
            else
            {
                // If it does, scrub out the old files to avoid
                // a partially written series.
                string[] files = Directory.GetFiles(SERIALIZATION_FOLDER, SERIALIZATION_PREFIX + name + "*");

                foreach (string file in files)
                    File.Delete(file);
            }

            StreamWriter output = new StreamWriter(SERIALIZATION_FOLDER + SERIALIZATION_PREFIX + name + SERIALIZATION_SUFFIX);
            StreamWriter outputCsv = new StreamWriter(SERIALIZATION_FOLDER + SERIALIZATION_PREFIX + name + String.Format("{0:000}", csvIndex) + SERIALIZATION_CSV_SUFFIX);
            bool currentlyWritingCsv = true;

            if (SkeletonDataType.ArmAngles == type)
            {
                foreach (Dictionary<Motor, List<KeyValuePair<DateTime, float>>> d in angles)
                {
                    if (!currentlyWritingCsv)
                        outputCsv = new StreamWriter(SERIALIZATION_FOLDER + SERIALIZATION_PREFIX + name + String.Format("{0:000}", csvIndex) + SERIALIZATION_CSV_SUFFIX);

                    output.WriteLine(SERIALIZATION_COMMENT + " [TIMESTAMP] LShoulderRoll LShoulderPitch LElbowRoll LElbowYaw RShoulderRoll RShoulderPitch RElbowRoll RElbowYaw");
                    outputCsv.WriteLine(SERIALIZATION_CSV_COMMENT + " [TIMESTAMP], LShoulderRoll, LShoulderPitch, LElbowRoll, LElbowYaw, RShoulderRoll, RShoulderPitch, RElbowRoll, RElbowYaw");

                    // Write this dictionary's series of angles.
                    for (int line = 0; line < d[Motor.LShoulderRoll].Count; line++)
                    {
                        DateTime date = d[Motor.LShoulderRoll][line].Key;
                        string timestamp = String.Format("[{0:0000}-{1:00}-{2:00} {3:00}:{4:00}:{5:00}.{6:000}] ",
                            date.Year, date.Month, date.Day, date.Hour, date.Minute, date.Second, date.Millisecond);

                        // Write to human-readable .txt file.
                        output.Write(timestamp);
                        output.Write(String.Format("{0:0.000} ", d[Motor.LShoulderRoll][line].Value));
                        output.Write(String.Format("{0:0.000} ", d[Motor.LShoulderPitch][line].Value));
                        output.Write(String.Format("{0:0.000} ", d[Motor.LElbowRoll][line].Value));
                        output.Write(String.Format("{0:0.000} ", d[Motor.LElbowYaw][line].Value));
                        output.Write(String.Format("{0:0.000} ", d[Motor.RShoulderRoll][line].Value));
                        output.Write(String.Format("{0:0.000} ", d[Motor.RShoulderPitch][line].Value));
                        output.Write(String.Format("{0:0.000} ", d[Motor.RElbowRoll][line].Value));
                        output.WriteLine(String.Format("{0:0.000}", d[Motor.RElbowYaw][line].Value));

                        // Write to comma-separated values (.csv) file for MATLAB purposes.
                        outputCsv.Write(date.ToFileTime());
                        outputCsv.Write(String.Format(", {0:0.000}, ", d[Motor.LShoulderRoll][line].Value));
                        outputCsv.Write(String.Format("{0:0.000}, ", d[Motor.LShoulderPitch][line].Value));
                        outputCsv.Write(String.Format("{0:0.000}, ", d[Motor.LElbowRoll][line].Value));
                        outputCsv.Write(String.Format("{0:0.000}, ", d[Motor.LElbowYaw][line].Value));
                        outputCsv.Write(String.Format("{0:0.000}, ", d[Motor.RShoulderRoll][line].Value));
                        outputCsv.Write(String.Format("{0:0.000}, ", d[Motor.RShoulderPitch][line].Value));
                        outputCsv.Write(String.Format("{0:0.000}, ", d[Motor.RElbowRoll][line].Value));
                        outputCsv.WriteLine(String.Format("{0:0.000}", d[Motor.RElbowYaw][line].Value));
                    }

                    // Notate the completion of this motion.
                    output.WriteLine(RECORDING_DELIMETER);
                    output.WriteLine();

                    outputCsv.Close();
                    csvIndex++;
                    currentlyWritingCsv = false;
                }
            }
            else
            {
                foreach (List<KeyValuePair<DateTime, Position>> l in positions)
                {
                    if (!currentlyWritingCsv)
                    {
                        outputCsv = new StreamWriter(SERIALIZATION_FOLDER + SERIALIZATION_PREFIX + name + String.Format("{0:000}", csvIndex) + SERIALIZATION_CSV_SUFFIX);
                    }

                    output.WriteLine(SERIALIZATION_COMMENT + " [TIMESTAMP] RelativeX RelativeY RelativeZ");
                    outputCsv.WriteLine(SERIALIZATION_CSV_COMMENT + " [TIMESTAMP], RelativeX, RelativeY, RelativeZ");

                    // Write this dictionary's series of angles.
                    for (int line = 0; line < l.Count; line++)
                    {
                        DateTime date = l[line].Key;
                        string timestamp = String.Format("[{0:0000}-{1:00}-{2:00} {3:00}:{4:00}:{5:00}.{6:000}] ",
                            date.Year, date.Month, date.Day, date.Hour, date.Minute, date.Second, date.Millisecond);

                        // Write to human-readable .txt file.
                        output.Write(timestamp);
                        output.Write(String.Format("{0:0.000} ", l[line].Value.x));
                        output.Write(String.Format("{0:0.000} ", l[line].Value.y));
                        output.WriteLine(String.Format("{0:0.000}", l[line].Value.z));

                        // Write to comma-separated values (.csv) file for MATLAB purposes.
                        outputCsv.Write(date.ToFileTime());
                        outputCsv.Write(String.Format(", {0:0.000}, ", l[line].Value.x));
                        outputCsv.Write(String.Format("{0:0.000}, ", l[line].Value.y));
                        outputCsv.WriteLine(String.Format("{0:0.000}", l[line].Value.z));
                    }

                    // Notate the completion of this motion.
                    output.WriteLine(RECORDING_DELIMETER);
                    output.WriteLine();

                    outputCsv.Close();
                    csvIndex++;
                    currentlyWritingCsv = false;
                }
            }

            output.Close();
            outputCsv.Close();
        }

        /// <summary>
        /// Load the skeleton data with this name from disk.
        /// 
        /// Preconditions:
        /// * This SkeletonDataParser object must have been initialized with
        /// the proper recording type as defined in 'type'.
        /// * The file must be properly formatted.
        /// </summary>
        public void LoadSkeletonData()
        {
            if (currentlyCapturing)
                throw new Exception("Currently capturing data.");

            using (StreamReader input = new StreamReader(SERIALIZATION_FOLDER + SERIALIZATION_PREFIX + name + SERIALIZATION_SUFFIX))
            {
                string line;

                while ((line = input.ReadLine()) != null)
                {
                    if (!currentlyCapturing && line.StartsWith("["))
                        BeginMotionCapture();

                    if (line.StartsWith(RECORDING_DELIMETER))
                    {
                        FinishMotionCapture();
                        continue;
                    }

                    if (line.Equals("") || line.StartsWith(SERIALIZATION_COMMENT))
                        continue;

                    string[] words = line.Split(' ');

                    DateTime date = new DateTime(Int32.Parse(words[0].Substring(1, 4)),
                        Int32.Parse(words[0].Substring(6, 2)),
                        Int32.Parse(words[0].Substring(9, 2)),
                        Int32.Parse(words[1].Substring(0, 2)),
                        Int32.Parse(words[1].Substring(3, 2)),
                        Int32.Parse(words[1].Substring(6, 2)),
                        Int32.Parse(words[1].Substring(9, 3)));

                    if (SkeletonDataType.ArmAngles == type)
                    {
                        Dictionary<Motor, float> angles = new Dictionary<Motor, float>();

                        angles[Motor.LShoulderRoll] = float.Parse(words[2]);
                        angles[Motor.LShoulderPitch] = float.Parse(words[3]);
                        angles[Motor.LElbowRoll] = float.Parse(words[4]);
                        angles[Motor.LElbowYaw] = float.Parse(words[5]);
                        angles[Motor.RShoulderRoll] = float.Parse(words[6]);
                        angles[Motor.RShoulderPitch] = float.Parse(words[7]);
                        angles[Motor.RElbowRoll] = float.Parse(words[8]);
                        angles[Motor.RElbowYaw] = float.Parse(words[9]);

                        AppendArmJointAngles(angles, date, true);
                    }
                    else
                    {
                        Position p;
                        p.x = float.Parse(words[2]);
                        p.y = float.Parse(words[3]);
                        p.z = float.Parse(words[4]);

                        AppendTorsoPosition(p, date, true);
                    }
                }

                if (currentlyCapturing)
                    FinishMotionCapture();
            }
        }
    }
}
